/*
 * Copyright 2016 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "mapping/map_builder_interface.h"

namespace cartographer {
namespace mapping {

MapBuilderInterface::MapBuilderInterface(const Eigen::Matrix4f& Tvl, const int trajectory_id, std::shared_ptr<Publisher> publisher_ptr) : Tvl_(Tvl) {
    global_trajectory_builder_ptr_ = std::make_shared<GlobalTrajectoryBuilder>(trajectory_id, publisher_ptr);

    publisher_ptr_ = publisher_ptr;
}

void MapBuilderInterface::HandleOdometry(const std::string& sensor_id,
                                         const sensor::OdometryData& odometry_data) {
    global_trajectory_builder_ptr_->AddSensorData(sensor_id, odometry_data);
}

void MapBuilderInterface::HandleImu(const std::string& sensor_id,
                                    const sensor::ImuData& imu_data) {
    global_trajectory_builder_ptr_->AddSensorData(sensor_id, imu_data);
}

void MapBuilderInterface::HandleLaserScan(
      const std::string& sensor_id, common::Time time,
      const sensor::PointCloudWithIntensities& points) {

  // LOG(INFO) << " points.points ========= : " << points.points.size() << std::endl;
  // LOG(INFO) << " time ========= : " << time << std::endl;

  if (points.points.empty()) {
    return;
  }

  // CHECK_LE: 小于等于
  CHECK_LE(points.points.back().time, 0.f);
  // TODO(gaschler): Use per-point time instead of subdivisions.

  // LOG(INFO) << " num_subdivisions_per_laser_scan_ ========= : " << num_subdivisions_per_laser_scan_ << std::endl;

  // 意为一帧雷达数据被分成几次处理, 一般将这个参数设置为1
  for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) {
    const size_t start_index =
        points.points.size() * i / num_subdivisions_per_laser_scan_;
    const size_t end_index =
        points.points.size() * (i + 1) / num_subdivisions_per_laser_scan_;
    
    // LOG(INFO) << " start_index ========= : " << start_index << std::endl;
    // LOG(INFO) << " end_index ========= : " << end_index << std::endl;

    // 生成分段的点云
    sensor::TimedPointCloud subdivision(
        points.points.begin() + start_index, points.points.begin() + end_index);
    if (start_index == end_index) {
      continue;
    }
    const double time_to_subdivision_end = subdivision.back().time;
    // `subdivision_time` is the end of the measurement so sensor::Collator will
    // send all other sensor data first.
    const common::Time subdivision_time =
        time + common::FromSeconds(time_to_subdivision_end);

    auto it = sensor_to_previous_subdivision_time_.find(sensor_id);

    // LOG(INFO) << " sensor_to_previous_subdivision_time_ ========= : " << sensor_to_previous_subdivision_time_.size() << std::endl;
    // LOG(INFO) << " time ========= : " << time << std::endl;
    // LOG(INFO) << " it->second ========= : " << it->second << std::endl;
    // LOG(INFO) << " subdivision_time ========= : " << subdivision_time << std::endl;
    // LOG(INFO) << " time_to_subdivision_end ========= : " << time_to_subdivision_end << std::endl;

    if (it != sensor_to_previous_subdivision_time_.end() &&
        // 上一段点云的时间不应该大于等于这一段点云的时间
        it->second >= subdivision_time) {
      LOG(WARNING) << "Ignored subdivision of a LaserScan message from sensor "
                   << sensor_id << " because previous subdivision time "
                   << it->second << " is not before current subdivision time "
                   << subdivision_time;
      continue;
    }
    // 更新对应sensor_id的时间戳
    sensor_to_previous_subdivision_time_[sensor_id] = subdivision_time;
    
    // 检查点云的时间
    for (auto& point : subdivision) {
      point.time -= time_to_subdivision_end;
    }
    CHECK_EQ(subdivision.back().time, 0.f);
    // 将分段后的点云 subdivision 传入 trajectory_builder_
    HandleRangefinder(sensor_id, subdivision_time, subdivision);
  } // for 
}

void MapBuilderInterface::HandleRangefinder(const std::string& sensor_id,
                                            common::Time time,
                                            const sensor::TimedPointCloud& ranges) {
  // todo: maybe Tlv?
  Eigen::Matrix3f R = Tvl_.block<3,3>(0,0);
  Eigen::Vector3f t = Tvl_.block<3,1>(0,3); 
  transform::Rigid3f ext_pose = transform::Rigid3f(t, Eigen::Quaternionf(R));
  sensor::TimedPointCloud trans_point_cloud = sensor::TransformTimedPointCloud(ranges, ext_pose);
  
  // for(auto point : trans_point_cloud) {
  //   LOG(INFO) << " point ========= : " << point.position.transpose() << std::endl;
  // }
  
  // LOG(INFO) << " trans_point_cloud : " << trans_point_cloud.size() << std::endl;
  
  global_trajectory_builder_ptr_->AddSensorData(
        sensor_id, sensor::TimedPointCloudData{
                       time, 
                       Eigen::Vector3f::Zero(),
                       // 将点云从雷达坐标系下转到tracking_frame坐标系系下
                       trans_point_cloud}); // 强度始终为空

}

}  // namespace mapping
}  // namespace cartographer
